/*
  class for copmutation of stream function of 2D velocity field
*/
#include "fem.h"
#include "output.h"

class C3DFlowProblem : public CStateProblem
{
 public:
  virtual void afterReadConfig();
  void calculateToF();
};

class C3DToFProblem : public CStateProblem
{
 private:
  int v_type;
  CIntPoint *vip3d;
  CIntPoint *vip2d;
  CIntPoint *vip1d;
  CVec *vq;
  CVar vx, vy, vz;
 public:
  void afterLoadMesh();
  void afterEvaluateVolumeComponents(int elem, int qp);
  void afterEvaluateFaceComponents(int elem, int qp);
  void setVelocity(int type, CVec *_vq, CIntPoint *ip3d, CIntPoint *ip2d, CIntPoint *ip1d);
  void setMesh(C3DMesh *m) { loadMesh(m->getType(), m->fileName().c_str()); };
  bool useVolumeIntegral() { return true; };
  bool useFaceIntegral() { return true; };
  bool useEdgeIntegral() { return false; };
  CVar volumeIntegral(int comp, int elem, int qp);
  CVar faceIntegral(int comp, int elem, int parent);
  
  friend class C3DFlowProblem;
};


C3DToFProblem SFP;


void C3DFlowProblem::afterReadConfig()
{
  SFP.setMesh(mesh);
//  SFP.afterLoadMesh();
  SFP.registerQuadratures(Q3d, Q2d, Q1d);
  SFP.setVelocity(FE_LIN, &q, U[0]->ip3d, U[0]->ip2d, U[0]->ip1d);
  SFP.params["savebase"] = params["savebase"] + "_tof";
  
  string bctag;
  for (map<int,string>::iterator bcname=mesh->beginPhysicalNames(); bcname!=mesh->endPhysicalNames(); bcname++)
  {
    bctag = "bc[" + bcname->second + ",tof]";
    if (params.find(bctag) != params.end())
      SFP.params["bc[" + bcname->second + ",0]"] = params[bctag];
  }
  SFP.addScalar("tof", FE_LIN_DISC);
}

void C3DFlowProblem::calculateToF()
{
  double xnorm;
  log(W_PROG, "\nCalculating time of flight...\n------------------------------\n");
  SFP.prepare();
  SFP.assemble();
  SFP.solve(xnorm, 1.0);
  SFP.saveVTKview();
  SFP.saveSolution();
}




void C3DToFProblem::afterLoadMesh()
{
	mesh->findChildrenParents(true);
}


void C3DToFProblem::setVelocity(int type, CVec *_vq, CIntPoint *ip3d, CIntPoint *ip2d, CIntPoint *ip1d)
{
	v_type = type;
	vq = _vq;
	vip3d = ip3d;
	vip2d = ip2d;
	vip1d = ip1d;
};


void C3DToFProblem::afterEvaluateVolumeComponents(int elem, int qp)
{
//  CVec g;
//  vip3d->calcE(elem, EVAL_FG);
//  vip3d->calcQP(qp, EVAL_FG);
//  vip3d->eval(*vq, elem, 0, vx, g, EVAL_FG);
//  vip3d->eval(*vq, elem, vip3d->getMeshNdof(), vy, g, EVAL_FG);
//  vip3d->eval(*vq, elem, 2*vip3d->getMeshNdof(), vz, g, EVAL_FG);

	int ind[4], offset;

  	for (int i=0; i<4; i++)
  		ind[i] = vip3d->mapDof(elem, i);

  	offset = vip3d->getMeshNdof();

  	vx = (*vq)(ind[0])*(1e0 - Q3d->x[qp] - Q3d->y[qp] - Q3d->z[qp])
  	   + (*vq)(ind[1])*Q3d->x[qp]
  	   + (*vq)(ind[2])*Q3d->y[qp]
  	   + (*vq)(ind[3])*Q3d->z[qp];

  	vy = (*vq)(offset+ind[0])*(1e0 - Q3d->x[qp] - Q3d->y[qp] - Q3d->z[qp])
  	   + (*vq)(offset+ind[1])*Q3d->x[qp]
  	   + (*vq)(offset+ind[2])*Q3d->y[qp]
	   + (*vq)(offset+ind[3])*Q3d->z[qp];

  	vz = (*vq)(2*offset+ind[0])*(1e0 - Q3d->x[qp] - Q3d->y[qp] - Q3d->z[qp])
  	   + (*vq)(2*offset+ind[1])*Q3d->x[qp]
  	   + (*vq)(2*offset+ind[2])*Q3d->y[qp]
	   + (*vq)(2*offset+ind[3])*Q3d->z[qp];
}


void C3DToFProblem::afterEvaluateFaceComponents(int elem, int qp)
{
	int ind[3], offset;

	for (int i=0; i<3; i++)
		ind[i] = vip3d->mapDof(mesh->getFaceParentNo(elem, 0), mesh->getFaceParentNodeNo(elem, 0, i));

	offset = vip3d->getMeshNdof();

	vx = (*vq)(ind[0])*(1e0 - Q2d->x[qp] - Q2d->y[qp])
	   + (*vq)(ind[1])*Q2d->x[qp]
	   + (*vq)(ind[2])*Q2d->y[qp];

	vy = (*vq)(offset+ind[0])*(1e0 - Q2d->x[qp] - Q2d->y[qp])
	   + (*vq)(offset+ind[1])*Q2d->x[qp]
	   + (*vq)(offset+ind[2])*Q2d->y[qp];

	vz = (*vq)(2*offset+ind[0])*(1e0 - Q2d->x[qp] - Q2d->y[qp])
	   + (*vq)(2*offset+ind[1])*Q2d->x[qp]
	   + (*vq)(2*offset+ind[2])*Q2d->y[qp];

//  CVec g;
//  vip2d->calcE(elem, EVAL_FG);
//  vip2d->calcQP(qp, EVAL_FG);
//  vip2d->eval(*vq, elem, 0, vx, g, EVAL_FG);
//  vip2d->eval(*vq, elem, vip3d->getMeshNdof(), vy, g, EVAL_FG);
//  vip2d->eval(*vq, elem, 2*vip3d->getMeshNdof(), vz, g, EVAL_FG);
}


CVar C3DToFProblem::volumeIntegral(int comp, int elem, int qp)
{
	return (U[0]->grad|gphi)*0e2 - (vx*gphi(0) + vy*gphi(1) + vz*gphi(2))*U[0]->val - phi*1;
}


CVar C3DToFProblem::faceIntegral(int comp, int elem, int parent)
{
	CVar Uupwind = 0;

	// compute barycenter of parent element no. 0
	double c[3];
	for (int j=0; j<3; j++) c[j] = 0;
	for (int i=0; i<4; i++)
	{
		for (int j=0; j<3; j++)
			c[j] += mesh->getCoord(mesh->getVolumeNode(mesh->getFaceParentNo(elem, 0), i), j).getVal() / 4;
	}

	// compute vector from the face to the barycenter
	double vp0[3];
	for (int j=0; j<3; j++)
		vp0[j] = c[j] - mesh->getCoord(mesh->getFaceNode(elem, 0), j).getVal();

	// compute normal vector to the face
	CVec nv, t1, t2;
	double n[3];
	mesh->calculateFaceNormal(elem, nv, t1, t2);
	for (int j=0; j<3; j++) n[j] = nv(j).getVal();

	// correct direction of the normal vector to point outside of element no. 0
	if (n[0]*vp0[0] + n[1]*vp0[1] + n[2]*vp0[2] > 0)
		for (int j=0; j<3; j++) n[j] = -n[j];

	// compute the flux coming from element 0
	double flux = vx.getVal()*n[0] + vy.getVal()*n[1] + vz.getVal()*n[2];

	if (mesh->getFaceNparents(elem) == 1)
	{
		if (flux >= 0)
			Uupwind = U[0]->dg_val[0];
	}
	else
	{
		if (flux >= 0)
			Uupwind = U[0]->dg_val[0];
		else
			Uupwind = U[0]->dg_val[1];

	}
    return Uupwind*phi*flux*(parent==0?1:-1);
}


